/*
 * forage.java
 */

import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

import EDU.gatech.cc.is.abstractrobot.SimpleN150;
import EDU.gatech.cc.is.util.Units;
import EDU.gatech.cc.is.util.Vec2;


public class Skills extends forage {
	
	// =================================================================
	// =================== BEHAVIORS ===================================
	// =================================================================

	// === WORLD MODEL =================================================
	static int leaderId=0;
	static Set<Integer> seeLeader = new HashSet<Integer>();
	static int teamSize=0;

	Vec2 goal = null;
	Vec2 obstacle = null;
	
	static WMFormation.Team team = null;
	WMFormation.Private wm = null;

	// === TASKS =======================================================
	class Idle extends BT.Task {
		public BT.STATUS step() {
			return BT.STATUS.Progress;
		}

	}

	// === --- PERCEPTION ----------------------------------------------
	
	class TrackNeighbors extends BT.Task {
		class Teammate{
			Vec2D pos = null;
			int id = -1;
			double heading=0;
			Vec2D vectorToLeader;
		}
		Teammate[] teammates = null;
		
		//final double fov = Vec2D.d2r(90); //field of view
		final double fov = Vec2D.d2r(360); //field of view
		final double vd = 1000; //view distance
		final double near = 0.1;
		
		public BT.STATUS step() {
			teammates = getTeammates();
			
			identifyTeammates();
			
			filterTeammatesOnFOV();
			
			printTeammates();
			
			updateKnowladge();
			
			checkIfISeeLeader();
			checkIfISeeAnchor();
			
			return BT.STATUS.Progress;
		}

// *** Get teammates locations by simulation function (with noise)		
//		Teammate[] getTeammates(){
//			Vec2[] ts = bot.getTeammates(timestamp);
//			Teammate[] tss = new Teammate[ts.length]
//			for(int i=0;i<ts.length;i++) tss[i].pos = new Vec2D(ts[i]);
//			return tss;
//		}

// *** Get teammates locations from related calculations of abs. positions (without noise)		
		Teammate[] getTeammates(){
			ArrayList<WMFormation.Knowladge.RobotIdentifier> ts = WMFormation.knowladge.getTeammates();
			Teammate[] v = new Teammate[ts.size()-1];
			for(int i=0, j=0;i<ts.size();i++){
				if(ts.get(i).id == bot.getID())continue;
				v[j] = new Teammate();
				v[j].pos = ts.get(i).position.toLocal( myPos(), heading());
				j++;
			}
			return v;
		}
		
		void identifyTeammates() {
			for(Teammate t: teammates){
				WMFormation.Knowladge.RobotIdentifier ri = howIsIt(t.pos);
				if(ri==null){
					t.id=-1;
					continue;
				}
				t.id = ri.id;
				t.heading = ri.heading-heading();
				t.vectorToLeader = ri.vectorToLeader;
			}
		}
		
		WMFormation.Knowladge.RobotIdentifier howIsIt(Vec2D t){
			ArrayList<WMFormation.Knowladge.RobotIdentifier> robots = WMFormation.knowladge.getTeammates();
			Vec2D tm = t.toGlobal( myPos(), heading() );
			for(WMFormation.Knowladge.RobotIdentifier ri : robots){
				if(Vec2D.distance(ri.position, tm) < near){
					return ri;
				}
			}
			return null;
		}

		void filterTeammatesOnFOV() {
			double hfov = fov*0.5+0.0001;
			int i=0;
			for(int k=0; k<teammates.length; k++){
				Teammate t = teammates[k];
				if(t.id>=0 && Math.abs(t.pos.h())<=hfov && t.pos.r()<=vd){
					i++;
					//print("FILTER: teammate #"+k+" in FOV");
				}else{
					teammates[k]=null;
					//print("FILTER: teammate #"+k+" out of FOV");
					//print("FILTER:               "+(Math.abs(t.pos.h())<=hfov)+" "+(t.pos.r()<=vd)+": t.pos.h()="+Vec2D.r2d(t.pos.h())+", hfov="+Vec2D.r2d(hfov)+", t.pos.r()="+t.pos.r());
				}
			}
			Teammate[] filtered = new Teammate[i];
			int j=0;
			for(int k=0; k<teammates.length; k++)if(teammates[k]!=null && teammates[k].id>=0){
				filtered[j++]=teammates[k];
			}
			teammates = filtered;
		}

		void printTeammates(){
			print("Print team ("+teammates.length+") : ");
			int n=0;
			for(Teammate t: teammates){ n++;
				print("   "+n+") id="+t.id+", pos="+t.pos.toString()+", heading="+t.heading);
			}
		}
		
		void updateKnowladge(){
			WMFormation.Teammate[] wm_teammates = new WMFormation.Teammate[teammates.length];
			if(teammates.length==0) print("WARNING: teammates.length is ZIRO");
			for(int i=0;i<teammates.length;i++){
				wm_teammates[i] = new WMFormation.Teammate(teammates[i].pos, teammates[i].id, teammates[i].heading, teammates[i].vectorToLeader);
			}
			wm.setTeammates(wm_teammates);
		}
		
		void checkIfISeeLeader(){
			if(wm.getMyId() == wm.team.getLeaderId()){
				print("I am leader => I do see leader");
				wm.setSeeLeader(true);
			}else{
				boolean found=false;
				for(Teammate t: teammates){
					if(t.id == wm.team.getLeaderId()){
						print("I see leader");
						found = true;
						break;
					}
				}
				if(!found){
					print("I don't see leader");
				}
				wm.setSeeLeader(found);
			}
		}
		void checkIfISeeAnchor(){
			if(wm.getMyId() == wm.team.getLeaderId()){
				print("I am leader => I do see anchor");
				wm.setSeeAnchor(true);
			}else{
				boolean found=false;
				for(Teammate t: teammates){
					if(t.id == wm.getAnchorId()){
						wm.setSeeAnchor(true);
						print("I see anchor");
						found = true;
						break;
					}
				}
				if(!found){
					print("I don't see anchore");
				}
				wm.setSeeAnchor(found);
			}
		}
	}

	class TrackGoalPosition extends BT.Task {
		public BT.STATUS step() {
			if(bot.getID() == wm.team.getLeaderId()) return BT.STATUS.Progress;
			Map<Integer, WMFormation.Teammate> wm_teammates = wm.getTeammates();
			if(wm.getSeeLeader()){
				int lid = wm.team.getLeaderId();
				double h = heading();
				Vec2D pos = myPos();
				Vec2D leader = wm_teammates.get(lid).pos.toGlobal(pos, h);
				double leader_h = wm_teammates.get(lid).heading + h;
				Vec2D goal = wm.getGoalVectorToLeader().scale(-1);
				Vec2D goal_position = goal.toGlobal(leader, leader_h);
				Vec2D diff = goal_position.toLocal(pos, h);
				print("TRACK(L): me="+new Vec2D.Pose(pos,h)+", leader="+new Vec2D.Pose(leader,leader_h)+", goal="+goal_position+", dist_to_goal="+F(diff.len()));
				wm.setRealDistanceToGoal(diff.len());
				wm.setRealDistanceToLeader(wm_teammates.get(lid).pos.len());
			}else{
				wm.setRealDistanceToGoal(-1);
				wm.setRealDistanceToLeader(-1);
			}
			if(wm.getSeeAnchor()){
				int aid = wm.getAnchorId();
				
				Vec2D.Pose mypose = myPose();
				Vec2D goal = wm.getGoalVectorToLeader().scale(-1);

				Vec2D.Pose anchor_local = wm_teammates.get(aid).pose();
				Vec2D from_anchor_to_leader = wm_teammates.get(aid).vectorToLeader;
				Vec2D.Pose anchor = anchor_local.add(mypose);

				Vec2D.Pose leader = new Vec2D.Pose(from_anchor_to_leader, 0).add(anchor);
				Vec2D goal_position = goal.toGlobal(leader.loc, leader.heading);
				Vec2D diff = goal_position.toLocal(mypose.loc, mypose.heading);
				
				print("TRACK(A): me="+mypose+", leader="+leader+", anchor="+anchor+", goal="+goal_position+", dist_to_goal="+F(diff.len()));

				wm.setDistanceToGoal(diff.len());
				wm.setDistanceToAnchor(wm_teammates.get(aid).pos.len());
				wm.setDistanceToLeader(leader.loc.sub(mypose.loc).len());
			}else{
				wm.setDistanceToGoal(-1);
				wm.setDistanceToAnchor(-1);
				wm.setDistanceToLeader(-1);				
			}
			return BT.STATUS.Progress;
		}
	}
	
	// === --- CONDITIONS ----------------------------------------------
	class CheckGripper extends BT.Task {
		public BT.STATUS step() {
			if (bot.getObjectInGripper(timestamp) > -1) {
				bot.setGripperFingers(timestamp, 0);
				return BT.STATUS.True;
			}
			return BT.STATUS.Progress;
		}

	}

	class BumpDetection extends BT.Task {
		public double minDistance = SimpleN150.RADIUS * 1000;

		public BT.STATUS step() {
			final double BUMP_DISTANCE = SimpleN150.RADIUS * 1.5;
			Vec2D opponent = closestObstacle();
			// Vec2D opponent = closestRobot();
			if (opponent != null) {
				opponent = opponent.sub(myPos());
				if (minDistance > opponent.r()) {
					minDistance = opponent.r();
				}
				// print("R="+F(BUMP_DISTANCE)+",C="+F(opponent.r())+",M="+F(minDistance));
				if (opponent.r() < BUMP_DISTANCE) {
					// print("BUMPED:"+"C="+F(opponent.r())+",M="+F(minDistance));
					return BT.STATUS.False;
				}
			}
			return BT.STATUS.Progress;
		}
	}

	class SomebodyLostLeader extends BT.Condition {
		public SomebodyLostLeader(){super();}
		public SomebodyLostLeader(boolean not){super(not);}
		public SomebodyLostLeader(BT.LOOP l, boolean not){super(l,not);}
		public SomebodyLostLeader(BT.LOOP l){super(l);}
		public boolean check() {
			WMFormation.Private _wm = wm;
			WMFormation.Team _team = _wm.team;
			Set<Integer> setok = _team.getSetSeeLeader();
			Set<Integer> setteam = _team.getSetTeam();
			print("SomebodyLostLeader: check "+ setok.size()+" < "+setteam.size()+" result is "+(setok.size() < setteam.size()));
			return setok.size() < setteam.size();
		}
	}
	class SomebodyLostAnchor extends BT.Condition {
		public SomebodyLostAnchor(){super();}
		public SomebodyLostAnchor(boolean not){super(not);}
		public SomebodyLostAnchor(BT.LOOP l, boolean not){super(l,not);}
		public SomebodyLostAnchor(BT.LOOP l){super(l);}
		public boolean check() {
			WMFormation.Private _wm = wm;
			WMFormation.Team _team = _wm.team;
			Set<Integer> setok = _team.getSetSeeAnchor();
			Set<Integer> setteam = _team.getSetTeam();
			print("SomebodyLostAnchor: check "+ setok.size()+" < "+setteam.size()+" result is "+(setok.size() < setteam.size()));
			return setok.size() < setteam.size();
		}
	}
	class SomebodyLostOrFar extends BT.Condition {
		public SomebodyLostOrFar(){super();}
		public SomebodyLostOrFar(boolean not){super(not);}
		public SomebodyLostOrFar(BT.LOOP l, boolean not){super(l,not);}
		public SomebodyLostOrFar(BT.LOOP l){super(l);}
		public boolean check() {
			WMFormation.Private _wm = wm;
			WMFormation.Team _team = _wm.team;
			Set<Integer> setok = _team.getSetSeeAnchor();
			Set<Integer> setteam = _team.getSetTeam();
			boolean lost = setok.size() < setteam.size(); 
			double dist_to_goal = _wm.getRealDistanceToGoal();
			boolean far = true;//dist_to_goal > 0.5;
			print("SomebodyLostOrFar : lost="+lost+", far="+far);
			return lost || far;
		}
		public BT.STATUS step(){
			boolean check = check();
			if(is_not) check = !check;
			if(loop == BT.LOOP.WHILE){
				if(check) return BT.STATUS.Progress;
				return BT.STATUS.True;
			}
			if(loop == BT.LOOP.UNTIL){
				if(!check){
					print("CONDITION LOOP CONTINUE");
					return BT.STATUS.Progress;
				}
				print("CONDITION LOOP STOPPED TRUE");
				return BT.STATUS.True;
			}
			return check? BT.STATUS.True: BT.STATUS.False; 
		}
	}
	
	class LeaderAllocation extends BT.Task{
		
		private BT.STATUS select(int i){
			error_code = i;
			return BT.STATUS.False;
		}
		
		public BT.STATUS step(){
			if( bot.getID() == wm.team.getLeaderId() ){ 
				wm.setIAmLeader(true);
				return select(1);
			}else{
				return select(2);
			}
		}
	}
	
	class IAmLeader extends BT.Condition{
		public IAmLeader(boolean not){
			super(not);
		}
		public IAmLeader(){
			super();
		}
		public boolean check(){
			return wm.getIAmLeader();
		}
	}
	
	class ISeeLeader extends BT.Condition{
		public ISeeLeader(){
			super();
		}
		public ISeeLeader(boolean not){
			super(not);
		}
		public ISeeLeader(BT.LOOP loop, boolean not){
			super(loop, not);
		}
		public boolean check(){
			return wm.getSeeLeader();
		}
	}
	
	// === --- GOAL SELECTION ------------------------------------------

	class SearchObstacle extends BT.Task {
		public BT.STATUS step() {
			Vec2D pos = myPos();
			Vec2D closest = closestObstacle();
			obstacle = null;
			if (closest != null && closest.sub(pos).r() < 2) {
				obstacle = closest.Vec2();
				// System.out.println("obst="+obstacle);
			}
			return BT.STATUS.Progress;
		}
	}

	class SetForwardGoal extends BT.Task {
		Vec2 ggoal = null;

		public BT.STATUS step() {
			if (ggoal == null) {
				ggoal = myPos().add(Vec2D.polar(heading(), 10)).Vec2();// .rotated(Math.PI).Vec2();
				System.out.println(ggoal);
			}
			goal = copy(ggoal); // myPos().add (
								// Vec2D.fromVec2(ggoal).sub(myPos()).changedR(1)
								// ).Vec2();
			// System.out.println("cgoal="+goal.toString() +
			// ", mypos="+myPos().Vec2());
			return BT.STATUS.Progress;
		}
	}

	class SetWanderingGoal extends BT.Task {
		double dir = 1;
		final double rotate_step = 10 * Math.PI / 180f;
		Timer timer = new Timer(1000);

		public BT.STATUS step() {
			Vec2D mvec = Vec2D.polar(heading(), 10);
			if (goal != null) {
				mvec = new Vec2D(goal).sub(myPos());
			}
			if (timer.isTime() && inCases(1 / 10f) || goal == null) {
				// System.out.println("change direction : "+dir);
				dir = (inCases(2 / 10f) ? -dir : dir);
				mvec = mvec.rotated(rotate_step * dir);
				goal = myPos().add(mvec).Vec2();
			}
			return BT.STATUS.Progress;
		}
	}

	// === --- MOTION --------------------------------------------------

	class MoveToGoal extends BT.Task {

		BT.STATUS onNoGoal() {
			bot.setSpeed(timestamp, 0);
			return BT.STATUS.Progress;
		}

		double turnAngleToGoal() {
			return Vec2D.heading(
			// heading from current position to goal without current heading
					new Vec2D(goal).sub(myPos()).h() - heading());
		}

		double correntTurnAngleForAvoidObstacle(double tg) {
			// calculate vector from current position to goal (lenght=1)
			Vec2D totar = Vec2D.polar(tg, 2);
			// vector from current position to obstacle rotate 180 deg (opposite
			// vector)
			Vec2D avoid = Vec2D.fromVec2(obstacle).sub(myPos()).rotated(Math.PI);
			// change vector length by formula of potential field : 1/(2R^2)
			avoid.r(1 / Math.pow(avoid.r() * 2, 2));
			return totar.add(avoid).h();
		}

		public BT.STATUS step() {
			double speed = 0.5;
			if (goal == null) {
				return onNoGoal();
			}

			double tg = turnAngleToGoal();
			// System.out.println("heading = "+heading());
			// System.out.println("tg="+tg);

			if (obstacle != null) {
				Vec2D toObst = new Vec2D(obstacle).sub(myPos()).rotated(-heading());
				// System.out.println("OBST! = "+toObst.r()+","+toObst.h());
				if (toObst.r() < SimpleN150.RADIUS * 2 && Math.abs(toObst.h()) < Units.DegToRad(30)) {
					// System.out.println("ESTOP");
					speed = 0f;
				}
				tg = correntTurnAngleForAvoidObstacle(tg);
			}

			final double turnStep = 0.1;
			if (Math.abs(tg) > turnStep) {
				if (tg < 0)
					tg = -turnStep;
				if (tg > 0)
					tg = +turnStep;
			}

			System.out.println("tg=" + tg);
			bot_turn(tg);

			bot.setSpeed(timestamp, speed);

			return BT.STATUS.Progress;
		}

	}

	class Wander extends BT.Task {
		long turntime = 0;

		public BT.STATUS step() {
			bot.setSpeed(timestamp, 0.5);
			if (Math.random() > 0.7 && (timestamp - turntime) > 5000) {
				bot_turn(timestamp, Units.DegToRad(45 * (Math.random() > 0.5 ? -1 : 1)));
				turntime = timestamp;
			}
			return BT.STATUS.Progress;
		}

	}
	
	class Say extends BT.Task {
		String text;
		boolean andStop;
		public Say(String text, boolean andStop){
			this.text = text;
			this.andStop = andStop;
		}
		public Say(String text){
			this.text = text;
			this.andStop = false;
		}
		public BT.STATUS step() {
			print("Say: "+text);
			if(andStop) return BT.STATUS.True;
			return BT.STATUS.Progress;
		}
		@Override
		protected String taskName() {
			return super.taskName()+":"+text;
		}
	}

	public enum ForagingMission_TRAJECTORY{ RANDOM, LINEAR, CYCLE, LINEAR_WITH_STOP }
	class ForagingMission extends BT.Task {
		ForagingMission_TRAJECTORY traj;
		public ForagingMission(ForagingMission_TRAJECTORY t) {
			traj=t;
		}
		long count=0;
		public BT.STATUS step() {
			count++;
			switch (traj) {
			case RANDOM:
				forageTakeStep_1();
				break;
			case LINEAR:
				setSpeed( 0.5, 0);
				break;
			case CYCLE:
				setSpeed( 0.5, Vec2D.d2r(0.1));
				break;
			case LINEAR_WITH_STOP:
				if(count>1000)
					setSpeed(0,0);
				else
					setSpeed( 0.5, 0);
				break;
			}
			return BT.STATUS.Progress;
		}
		
		void setSpeed(double lin, double ang){
			double h = heading();
			bot.setSpeed(timestamp, lin);
			bot.setSteerHeading(timestamp, h+ ang);
			bot.setTurretHeading(timestamp, h+ ang);			
		}
	}

	class Stop extends BT.Task {
		public BT.STATUS step() {
			bot.setSpeed(timestamp, 0);
			return BT.STATUS.True;
		}
	}

	enum FOLLOW_TYPE{
		follow_leader_localization,
		follow_anchor_localization,
		follow_leader_bearing,
		follow_anchor_bearing
	};
	class Follow extends BT.Task{
		FOLLOW_TYPE follow_type = FOLLOW_TYPE.follow_leader_localization;
		public Follow(){}
		public Follow(FOLLOW_TYPE ft){
			follow_type = ft;
		}
		public BT.STATUS step(){ 
			try{
				switch(follow_type){
				case follow_leader_localization:
					return step_by_leader();
				case follow_anchor_localization:
					return step_by_anchore();
				case follow_leader_bearing:
					return step_by_angles_distance_to_leader();
					//return step_by_distance_angles_to_leader();
				case follow_anchor_bearing:
					return step_by_angles_distance_to_anchor();
				}
				
			}catch(Throwable e){
				print("Exception: "+e.toString());
				e.printStackTrace();
			}
			error_code=1010;
			return BT.STATUS.False;
		}
		
		private void moveRobot(Vec2D mv){ moveRobot(mv.t(), mv.r()); }
		private void moveRobot(double turn, double speed){
			final double angle = Vec2D.d2r(1);
			double t = Vec2D.polar(turn, 1).ang();
			double h = heading();
			double turet_heading = h+t;
			double steer_heading = h+(Math.abs(t)>Vec2D.d2r(10)? (t > 0 ? 10*angle : -10*angle) : t);
			speed = speed>0.01? speed : 0;
			bot.setTurretHeading(timestamp, turet_heading);
			bot.setSteerHeading(timestamp, steer_heading);
			bot.setSpeed(timestamp, speed);
		}
		private void stopRobot(){ moveRobot(0, 0); }
		
		public BT.STATUS step_by_leader(){
			if(wm.getSeeLeader()==false){
				stopRobot();
				return BT.STATUS.Progress;
			}
			Vec2D me = myPos();
			Map<Integer, WMFormation.Teammate> team = wm.getTeammates();
			int lid = wm.team.getLeaderId();
			if(team.containsKey(lid)){
				double heading = heading();
				Vec2D goal = wm.getGoalVectorToLeader().scale(-1);
				Vec2D leader_from_me = team.get(lid).pos;
				double leader_heading = team.get(lid).heading+heading;
				Vec2D leader = leader_from_me.toGlobal(me, heading);
				
				Vec2D move = leader.add(goal.rotated(leader_heading)).toLocal(me, heading);
				moveRobot(move);
			}
			
			return BT.STATUS.Progress;
		}
		public BT.STATUS step_by_anchore(){
			if( wm.getSeeAnchor()==false ){
				stopRobot();
				return BT.STATUS.Progress;
			}
			
			Vec2D me = myPos();
			Map<Integer, WMFormation.Teammate> team = wm.getTeammates();
			if(team.containsKey(wm.getAnchorId())){
				double heading = heading();
				Vec2D goal = wm.getGoalVectorToLeader().scale(-1);
				
				int aid= wm.getAnchorId();
				
				Vec2D anchor_from_me = team.get(aid).pos;
				double anchor_heading = team.get(aid).heading+heading;
				Vec2D leader_from_anchor = team.get(aid).vectorToLeader;
				
				Vec2D g_anchor = anchor_from_me.toGlobal(me,  heading);
				Vec2D g_leader = leader_from_anchor.toGlobal(g_anchor, anchor_heading);
				
				Vec2D move = g_leader.add(goal.rotated(anchor_heading)).toLocal(me, heading);
				
				
				moveRobot(move);
			}
			
			return BT.STATUS.Progress;
		}

		public BT.STATUS step_by_angles_distance_to_leader(){
			if(wm.getSeeLeader()==false){
				stopRobot();
				return BT.STATUS.Progress;
			}
			
			Map<Integer, WMFormation.Teammate> team = wm.getTeammates();
			
			Vec2D leader_from_me = team.get(wm.team.getLeaderId()).pos;
			Vec2D target_vector = wm.getGoalVectorToLeader();
			double move_ang = leader_from_me.ang() - target_vector.ang();
			double move_len = ( leader_from_me.len() - target_vector.len() )*2;
			
			moveRobot(move_ang, move_len);
			
			return BT.STATUS.Progress;
		}
		public BT.STATUS step_by_angles_distance_to_anchor(){
			if(wm.getSeeAnchor()==false){
				print("MOVE: stop");
				stopRobot();
				return BT.STATUS.Progress;
			}
			
			Map<Integer, WMFormation.Teammate> team = wm.getTeammates();
			
			int aid = wm.getAnchorId();
			
			Vec2D leader_from_anchor = team.get(aid).vectorToLeader;
			Vec2D anchor_from_me = team.get(aid).pos;
			Vec2D target_vector_to_leader = wm.getGoalVectorToLeader();
			Vec2D target_vector_to_anchor = target_vector_to_leader.sub(leader_from_anchor);
			
			
			double move_ang = anchor_from_me.ang() - target_vector_to_anchor.ang();
			double move_len = ( anchor_from_me.len() - target_vector_to_anchor.len() )*2;
			moveRobot(move_ang, move_len);
			
			return BT.STATUS.Progress;
		}
		
		public BT.STATUS step_by_distance_angles_to_leader(){
			if(wm.getSeeLeader()==false){
				stopRobot();
				return BT.STATUS.Progress;
			}
			
			Map<Integer, WMFormation.Teammate> team = wm.getTeammates();
			
			Vec2D leader_from_me = team.get(wm.team.getLeaderId()).pos;
			Vec2D target_vector = wm.getGoalVectorToLeader();
			double move_ang = Vec2D.polar(leader_from_me.ang() - target_vector.ang(), 1).h();
			double move_len = ( leader_from_me.len() - target_vector.len() )*2;
			if( move_len > 1){
				moveRobot(move_ang, 0);
			}else{
				moveRobot(leader_from_me.ang(), move_len);
			}
			
			return BT.STATUS.Progress;
		}
		
	}
	// === --- COORDINATION TRACKING -----------------------------------

	class StartConflict extends BT.Task {
		public BT.STATUS step() {
			print("StartConflict");
			bot.setSpeed(timestamp, 0);
			return BT.STATUS.True;
		}
	}

	class EndConflict extends BT.Task {
		public BT.STATUS step() {
			// print("EndCollision");
			return BT.STATUS.True;
		}
	}

	class SelectConflictSolution extends BT.Task {
		BT.STATUS select(int i) {
			error_code = i;
			return BT.STATUS.False;
		}

		public BT.STATUS step() {
			// print("SelectConflictSolution");
			return select(1);
		}
	}

	// === --- COORDINATION METHODS ------------------------------------

	// === --- --- FORAGING --------------------------------------------
	class Noise extends BT.Task {
		int stepcount = 0;
		final int stepcountlimit = 20;
		double randomDir = 0;

		public BT.STATUS step() {
			if (stepcount == 0)
				setup();

			print("NOISE:" + (stepcountlimit - stepcount));

			abstract_robot.setSpeed(timestamp, 1);
			stepcount++;

			if (stepcount > stepcountlimit) {
				cleanup();
				return BT.STATUS.True;
			}
			return BT.STATUS.Progress;
		}

		public void terminated() {
			cleanup();
		}

		public void setup() {
			randomDir = rnd(-45.0 * Math.PI / 180.0, +45.0 * Math.PI / 180.0);
			double h = bot.getSteerHeading(timestamp) + randomDir;
			abstract_robot.setSteerHeading(timestamp, h);
		}

		public void cleanup() {
			print("");
			stepcount = 0;
		}
	}

	class Ripel extends BT.Task {
		int stepcount = 0;
		final int stepcountlimit = 50;
		double randomDir = 0;

		public BT.STATUS step() {
			if (stepcount == 0)
				setup();

			print("Repel:" + (stepcountlimit - stepcount));

			abstract_robot.setSpeed(timestamp, 1);
			stepcount++;

			if (stepcount > stepcountlimit) {
				cleanup();
				return BT.STATUS.True;
			}
			return BT.STATUS.Progress;
		}

		public void terminated() {
			cleanup();
		}

		public void setup() {
			Vec2D obs = closestObstacle();
			if (obs != null)
				obs = myPos().sub(obs);

			abstract_robot.setSteerHeading(timestamp, obs.t());
			abstract_robot.setTurretHeading(timestamp, obs.t());
		}

		public void cleanup() {
			print("");
			stepcount = 0;
		}
	}

	class Aggression extends BT.Task {
		int stepcount = 0;
		final int stepcountlimit = 50;
		double randomDir = 0;
		int sleepTime = 0;

		public BT.STATUS step() {
			if (stepcount == 0)
				setup();
			stepcount++;

			if (stepcount < sleepTime) {
				print("Aggression:sleep=" + sleepTime + ", rest=" + (sleepTime - stepcount));
				return BT.STATUS.Progress;
			} else if (sleepTime == stepcount) {
				if (closestObstacle() == null || closestObstacle().sub(myPos()).r() > SimpleN150.RADIUS * 1.5) {
					print("Aggression: I am dominant");
					return BT.STATUS.True;
				} else {
					print("Aggression: I am depressed");
					return BT.STATUS.Progress;
				}
			}

			print("Aggression:repel=" + stepcountlimit + ", rest=" + (stepcountlimit + sleepTime - stepcount));

			abstract_robot.setSpeed(timestamp, 1);

			if (stepcount > stepcountlimit + sleepTime) {
				cleanup();
				return BT.STATUS.True;
			}
			return BT.STATUS.Progress;
		}

		public void terminated() {
			cleanup();
		}

		public void setup() {
			Vec2D obs = closestObstacle();
			if (obs != null)
				obs = myPos().sub(obs);

			abstract_robot.setSteerHeading(timestamp, obs.t());
			abstract_robot.setTurretHeading(timestamp, obs.t());

			sleepTime = (int) rnd(0, 50);
		}

		public void cleanup() {
			print("");
			stepcount = 0;
		}
	}

	// === --- --- FORMATION ------------------------------------------
	
	class TimedRandomMove extends BT.Task {
		private long counter=0;
		public BT.STATUS step(){
			counter++;
			if(counter>200)
				return BT.STATUS.True;
			return BT.STATUS.Progress;
		}
		public void terminated(){}
		public void setup(){}
		public void cleanup(){}
	}
	class RandomMoveUntilSolved extends BT.Task {
		public BT.STATUS step(){return BT.STATUS.Progress;}
		public void terminated(){}
		public void setup(){}
		public void cleanup(){}		
	}
	class ChangeLeader extends BT.Task {
		public BT.STATUS step(){return BT.STATUS.Progress;}
		public void terminated(){}
		public void setup(){}
		public void cleanup(){}		
	}
	class ChangeLeaderAndValidateLocation extends BT.Task {
		public BT.STATUS step(){return BT.STATUS.Progress;}
		public void terminated(){}
		public void setup(){}
		public void cleanup(){}		
	}
	
	// === EXECUTION ==================================================
	BT.Task behavior = null;

	public BT.LookupTable lkpTbl = new BT.LookupTable();

	
	protected void setupBehaviors(){
		
	}
	
	protected BT.Task loadTaskFromFile(String taskname, String filename){
		try {
			BTReader reader = new BTReader(new File(PHD_FOLDER + "/"+filename+".plan.xml").getAbsolutePath());
			BT.Task mission = reader.load(lkpTbl);
			lkpTbl.add(taskname, mission);
			return mission;
		} catch (IOException e) {
			e.printStackTrace();
		}
		return null;
	}
	protected void add(String nick, BT.Task task){
		lkpTbl.add(nick, task);
	}
	protected void add(BT.Task task){
		lkpTbl.add(task);
	}
	
	protected void onTeamTrialInit(){
		print("Team Trial init");
		super.onTeamTrialInit();
		team = new WMFormation.Team(null);
	}
	synchronized protected void onTrialInit() {
		if(wm!=null) return;
		print("Trial init");
		super.onTrialInit();
		wm = new WMFormation.Private(bot.getID(), team);
		behavior.reset();
	}

	public void configure() {
		super.configure();
		
		setupBehaviors();
		
		print("BT: \n" + behavior.toString());
	}

	// === --- STEPS --------------------------------------------------

	public int takeStep() {
		onTrialInit();
		super.controllerTakeStep();
		try{
			WMFormation.knowladge.update(new WMFormation.Knowladge.RobotIdentifier(bot.getID(), myPos(), heading(), wm.getGoalVectorToLeader()));
		}catch(Throwable e){
			print("Exception: "+e.toString());
			print("   knowladge = "+(WMFormation.knowladge==null?"null":"OK"));
			print("   bot = "+(bot==null?"null":"OK"));
			print("   wm = "+(wm==null?"null":"OK"));
			print("   wm.getGoalVectorToLeader() = "+(wm.getGoalVectorToLeader()==null?"null":"OK"));
			e.printStackTrace();
			System.exit(1);
		}

		behavior.timestamp = timestamp;
		behavior.step();

		return (CSSTAT_OK);
	}

}
